﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/b2PulleyJoint.h
// Revision   : r29
// Change-Date: 2009-11-28
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/b2PulleyJoint.cpp
// Revision   : r28
// Change-Date: 2009-11-23
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    /// <summary>
    /// Pulley joint definition. This requires two ground anchors,
    /// two dynamic body anchor points, max lengths for each side,
    /// and a pulley ratio.
    /// </summary>
    public class PulleyJointDef : JointDef
    {
	    public PulleyJointDef()
	    {
		    type = JointType.PULLEY;
		    groundAnchor1 = new Vector2(-1.0f, 1.0f);
		    groundAnchor2 = new Vector2(1.0f, 1.0f);
		    localAnchor1 = new Vector2(-1.0f, 0.0f);
		    localAnchor2 = new Vector2(1.0f, 0.0f);
		    length1 = 0.0f;
		    maxLength1 = 0.0f;
		    length2 = 0.0f;
		    maxLength2 = 0.0f;
		    ratio = 1.0f;
		    collideConnected = true;
	    }

        /// <summary>Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.</summary>
        public void Initialize(Body body1, Body body2,
                               Vector2 groundAnchor1, Vector2 groundAnchor2,
                               Vector2 anchor1, Vector2 anchor2,
                               float ratio)
        {
            this.body1 = body1;
            this.body2 = body2;
            this.groundAnchor1 = groundAnchor1;
            this.groundAnchor2 = groundAnchor2;
            localAnchor1 = body1.GetLocalPoint(anchor1);
            localAnchor2 = body2.GetLocalPoint(anchor2);
            Vector2 d1 = anchor1 - groundAnchor1;
            length1 = d1.Length();
            Vector2 d2 = anchor2 - groundAnchor2;
            length2 = d2.Length();
            this.ratio = ratio;
            System.Diagnostics.Debug.Assert(ratio > PhysicSettings.FLT_EPSILON);
            float C = length1 + ratio * length2;
            maxLength1 = C - ratio * PulleyJoint.minPulleyLength;
            maxLength2 = (C - PulleyJoint.minPulleyLength) / ratio;
        }

        /// <summary>The first ground anchor in world coordinates. This point never moves.</summary>
	    public Vector2 groundAnchor1;

        /// <summary>The second ground anchor in world coordinates. This point never moves.</summary>
        public Vector2 groundAnchor2;

        /// <summary>The local anchor point relative to body1's origin.</summary>
        public Vector2 localAnchor1;

        /// <summary>The local anchor point relative to body2's origin.</summary>
        public Vector2 localAnchor2;

        /// <summary>The a reference length for the segment attached to body1.</summary>
        public float length1;

        /// <summary>The maximum length of the segment attached to body1.</summary>
        public float maxLength1;

        /// <summary>The a reference length for the segment attached to body2.</summary>
        public float length2;

        /// <summary>The maximum length of the segment attached to body2.</summary>
        public float maxLength2;

        /// <summary>The pulley ratio, used to simulate a block-and-tackle.</summary>
        public float ratio;
    }

    /// <summary>The pulley joint is connected to two bodies and two fixed ground points.
    /// The pulley supports a ratio such that:
    /// length1 + ratio * length2 = constant
    /// Yes, the force transmitted is scaled by the ratio.
    /// The pulley also enforces a maximum length limit on both sides. This is
    /// useful to prevent one side of the pulley hitting the top.</summary>
    public class PulleyJoint : Joint
    {
        public static float minPulleyLength = 2.0f;

        public override Vector2 GetAnchorA()
        {
            return m_body1.GetWorldPoint(ref m_localAnchor1);
        }

        public override Vector2 GetAnchorB()
        {
            return m_body2.GetWorldPoint(ref m_localAnchor2);
        }

        public override Vector2 GetReactionForce(float inv_dt)
        {
            Vector2 P = m_impulse * m_u2;
            return inv_dt * P;
        }

        public override float GetReactionTorque(float inv_dt)
        {
            //B2_NOT_USED(inv_dt);
            return 0.0f;
        }

        /// <summary>Get the first ground anchor.</summary>
        public Vector2 GetGroundAnchor1()
        {
            return m_groundAnchor1;
        }

        /// <summary>Get the second ground anchor.</summary>
        public Vector2 GetGroundAnchor2()
        {
            return m_groundAnchor2;
        }

        /// <summary>Get the current length of the segment attached to body1.</summary>
        public float GetLength1()
        {
            Vector2 p = m_body1.GetWorldPoint(ref m_localAnchor1);
            Vector2 s = m_groundAnchor1;
            Vector2 d = p - s;
            return d.Length();
        }

        /// <summary>Get the current length of the segment attached to body2.</summary>
        public float GetLength2()
        {
            Vector2 p = m_body2.GetWorldPoint(ref m_localAnchor2);
            Vector2 s = m_groundAnchor2;
            Vector2 d = p - s;
            return d.Length();
        }

        /// <summary>Get the pulley ratio.</summary>
        public float GetRatio()
        {
            return m_ratio;
        }

        public PulleyJoint(PulleyJointDef def)
            : base(def)
        {
            m_groundAnchor1 = def.groundAnchor1;
            m_groundAnchor2 = def.groundAnchor2;
            m_localAnchor1 = def.localAnchor1;
            m_localAnchor2 = def.localAnchor2;

            System.Diagnostics.Debug.Assert(def.ratio != 0.0f);
            m_ratio = def.ratio;

            m_constant = def.length1 + m_ratio * def.length2;

            m_maxLength1 = System.Math.Min(def.maxLength1, m_constant - m_ratio * minPulleyLength);
            m_maxLength2 = System.Math.Min(def.maxLength2, (m_constant - minPulleyLength) / m_ratio);

            m_impulse = 0.0f;
            m_limitImpulse1 = 0.0f;
            m_limitImpulse2 = 0.0f;
        }

        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = m_body1;
            Body b2 = m_body2;

            Vector2 r1 = Math.Mul(b1.Transform.R, m_localAnchor1 - b1.LocalCenter);
            Vector2 r2 = Math.Mul(b2.Transform.R, m_localAnchor2 - b2.LocalCenter);

            Vector2 p1 = b1.m_sweep.c + r1;
            Vector2 p2 = b2.m_sweep.c + r2;

            Vector2 s1 = m_groundAnchor1;
            Vector2 s2 = m_groundAnchor2;

            // Get the pulley axes.
            m_u1 = p1 - s1;
            m_u2 = p2 - s2;

            float length1 = m_u1.Length();
            float length2 = m_u2.Length();

            if (length1 > PhysicSettings.LinearSlop)
            {
                m_u1 *= 1.0f / length1;
            }
            else
            {
                m_u1 = Vector2.Zero;
            }

            if (length2 > PhysicSettings.LinearSlop)
            {
                m_u2 *= 1.0f / length2;
            }
            else
            {
                m_u2 = Vector2.Zero;
            }

            float C = m_constant - length1 - m_ratio * length2;
            if (C > 0.0f)
            {
                m_state = LimitState.INACTIVE;
                m_impulse = 0.0f;
            }
            else
            {
                m_state = LimitState.AT_UPPER;
            }

            if (length1 < m_maxLength1)
            {
                m_limitState1 = LimitState.INACTIVE;
                m_limitImpulse1 = 0.0f;
            }
            else
            {
                m_limitState1 = LimitState.AT_UPPER;
            }

            if (length2 < m_maxLength2)
            {
                m_limitState2 = LimitState.INACTIVE;
                m_limitImpulse2 = 0.0f;
            }
            else
            {
                m_limitState2 = LimitState.AT_UPPER;
            }

            // Compute effective mass.
            float cr1u1 = Vector2Extensions.Cross(r1, m_u1);
            float cr2u2 = Vector2Extensions.Cross(r2, m_u2);

            m_limitMass1 = b1.m_invMass + b1.m_invI * cr1u1 * cr1u1;
            m_limitMass2 = b2.m_invMass + b2.m_invI * cr2u2 * cr2u2;
            m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2;
            System.Diagnostics.Debug.Assert(m_limitMass1 > PhysicSettings.FLT_EPSILON);
            System.Diagnostics.Debug.Assert(m_limitMass2 > PhysicSettings.FLT_EPSILON);
            System.Diagnostics.Debug.Assert(m_pulleyMass > PhysicSettings.FLT_EPSILON);
            m_limitMass1 = 1.0f / m_limitMass1;
            m_limitMass2 = 1.0f / m_limitMass2;
            m_pulleyMass = 1.0f / m_pulleyMass;

            if (step.warmStarting)
            {
                // Scale impulses to support variable time steps.
                m_impulse *= step.dtRatio;
                m_limitImpulse1 *= step.dtRatio;
                m_limitImpulse2 *= step.dtRatio;

                // Warm starting.
                Vector2 P1 = -(m_impulse + m_limitImpulse1) * m_u1;
                Vector2 P2 = (-m_ratio * m_impulse - m_limitImpulse2) * m_u2;
                b1.m_linearVelocity += b1.m_invMass * P1;
                b1.m_angularVelocity += b1.m_invI * Vector2Extensions.Cross(r1, P1);
                b2.m_linearVelocity += b2.m_invMass * P2;
                b2.m_angularVelocity += b2.m_invI * Vector2Extensions.Cross(r2, P2);
            }
            else
            {
                m_impulse = 0.0f;
                m_limitImpulse1 = 0.0f;
                m_limitImpulse2 = 0.0f;
            }
        }

        internal override void SolveVelocityConstraints(TimeStep step)
        {
            //B2_NOT_USED(step);

            Body b1 = m_body1;
            Body b2 = m_body2;

            Vector2 r1 = Math.Mul(b1.Transform.R, m_localAnchor1 - b1.LocalCenter);
            Vector2 r2 = Math.Mul(b2.Transform.R, m_localAnchor2 - b2.LocalCenter);

            if (m_state == LimitState.AT_UPPER)
            {
                Vector2 v1 = b1.m_linearVelocity + Vector2Extensions.Cross(b1.m_angularVelocity, r1);
                Vector2 v2 = b2.m_linearVelocity + Vector2Extensions.Cross(b2.m_angularVelocity, r2);

                float Cdot = -Vector2.Dot(m_u1, v1) - m_ratio * Vector2.Dot(m_u2, v2);
                float impulse = m_pulleyMass * (-Cdot);
                float oldImpulse = m_impulse;
                m_impulse = System.Math.Max(0.0f, m_impulse + impulse);
                impulse = m_impulse - oldImpulse;

                Vector2 P1 = -impulse * m_u1;
                Vector2 P2 = -m_ratio * impulse * m_u2;
                b1.m_linearVelocity += b1.m_invMass * P1;
                b1.m_angularVelocity += b1.m_invI * Vector2Extensions.Cross(r1, P1);
                b2.m_linearVelocity += b2.m_invMass * P2;
                b2.m_angularVelocity += b2.m_invI * Vector2Extensions.Cross(r2, P2);
            }

            if (m_limitState1 == LimitState.AT_UPPER)
            {
                Vector2 v1 = b1.m_linearVelocity + Vector2Extensions.Cross(b1.m_angularVelocity, r1);

                float Cdot = -Vector2.Dot(m_u1, v1);
                float impulse = -m_limitMass1 * Cdot;
                float oldImpulse = m_limitImpulse1;
                m_limitImpulse1 = System.Math.Max(0.0f, m_limitImpulse1 + impulse);
                impulse = m_limitImpulse1 - oldImpulse;

                Vector2 P1 = -impulse * m_u1;
                b1.m_linearVelocity += b1.m_invMass * P1;
                b1.m_angularVelocity += b1.m_invI * Vector2Extensions.Cross(r1, P1);
            }

            if (m_limitState2 == LimitState.AT_UPPER)
            {
                Vector2 v2 = b2.m_linearVelocity + Vector2Extensions.Cross(b2.m_angularVelocity, r2);

                float Cdot = -Vector2.Dot(m_u2, v2);
                float impulse = -m_limitMass2 * Cdot;
                float oldImpulse = m_limitImpulse2;
                m_limitImpulse2 = System.Math.Max(0.0f, m_limitImpulse2 + impulse);
                impulse = m_limitImpulse2 - oldImpulse;

                Vector2 P2 = -impulse * m_u2;
                b2.m_linearVelocity += b2.m_invMass * P2;
                b2.m_angularVelocity += b2.m_invI * Vector2Extensions.Cross(r2, P2);
            }
        }

        internal override bool SolvePositionConstraints(float baumgarte)
        {
            //B2_NOT_USED(baumgarte);

            Body b1 = m_body1;
            Body b2 = m_body2;

            Vector2 s1 = m_groundAnchor1;
            Vector2 s2 = m_groundAnchor2;

            float linearError = 0.0f;

            if (m_state == LimitState.AT_UPPER)
            {
                Vector2 r1 = Math.Mul(b1.Transform.R, m_localAnchor1 - b1.LocalCenter);
                Vector2 r2 = Math.Mul(b2.Transform.R, m_localAnchor2 - b2.LocalCenter);

                Vector2 p1 = b1.m_sweep.c + r1;
                Vector2 p2 = b2.m_sweep.c + r2;

                // Get the pulley axes.
                m_u1 = p1 - s1;
                m_u2 = p2 - s2;

                float length1 = m_u1.Length();
                float length2 = m_u2.Length();

                if (length1 > PhysicSettings.LinearSlop)
                {
                    m_u1 *= 1.0f / length1;
                }
                else
                {
                    m_u1 = Vector2.Zero;
                }

                if (length2 > PhysicSettings.LinearSlop)
                {
                    m_u2 *= 1.0f / length2;
                }
                else
                {
                    m_u2 = Vector2.Zero;
                }

                float C = m_constant - length1 - m_ratio * length2;
                linearError = System.Math.Max(linearError, -C);

                C = MathHelper.Clamp(C + PhysicSettings.LinearSlop, -PhysicSettings.MaxLinearCorrection, 0.0f);
                float impulse = -m_pulleyMass * C;

                Vector2 P1 = -impulse * m_u1;
                Vector2 P2 = -m_ratio * impulse * m_u2;

                b1.m_sweep.c += b1.m_invMass * P1;
                b1.m_sweep.a += b1.m_invI * Vector2Extensions.Cross(r1, P1);
                b2.m_sweep.c += b2.m_invMass * P2;
                b2.m_sweep.a += b2.m_invI * Vector2Extensions.Cross(r2, P2);

                b1.SynchronizeTransform();
                b2.SynchronizeTransform();
            }

            if (m_limitState1 == LimitState.AT_UPPER)
            {
                Vector2 r1 = Math.Mul(b1.Transform.R, m_localAnchor1 - b1.LocalCenter);
                Vector2 p1 = b1.m_sweep.c + r1;

                m_u1 = p1 - s1;
                float length1 = m_u1.Length();

                if (length1 > PhysicSettings.LinearSlop)
                {
                    m_u1 *= 1.0f / length1;
                }
                else
                {
                    m_u1 = Vector2.Zero;
                }

                float C = m_maxLength1 - length1;
                linearError = System.Math.Max(linearError, -C);
                C = MathHelper.Clamp(C + PhysicSettings.LinearSlop, -PhysicSettings.MaxLinearCorrection, 0.0f);
                float impulse = -m_limitMass1 * C;

                Vector2 P1 = -impulse * m_u1;
                b1.m_sweep.c += b1.m_invMass * P1;
                b1.m_sweep.a += b1.m_invI * Vector2Extensions.Cross(r1, P1);

                b1.SynchronizeTransform();
            }

            if (m_limitState2 == LimitState.AT_UPPER)
            {
                Vector2 r2 = Math.Mul(b2.Transform.R, m_localAnchor2 - b2.LocalCenter);
                Vector2 p2 = b2.m_sweep.c + r2;

                m_u2 = p2 - s2;
                float length2 = m_u2.Length();

                if (length2 > PhysicSettings.LinearSlop)
                {
                    m_u2 *= 1.0f / length2;
                }
                else
                {
                    m_u2 = Vector2.Zero;
                }

                float C = m_maxLength2 - length2;
                linearError = System.Math.Max(linearError, -C);
                C = MathHelper.Clamp(C + PhysicSettings.LinearSlop, -PhysicSettings.MaxLinearCorrection, 0.0f);
                float impulse = -m_limitMass2 * C;

                Vector2 P2 = -impulse * m_u2;
                b2.m_sweep.c += b2.m_invMass * P2;
                b2.m_sweep.a += b2.m_invI * Vector2Extensions.Cross(r2, P2);

                b2.SynchronizeTransform();
            }

            return linearError < PhysicSettings.LinearSlop;
        }

        internal Vector2 m_groundAnchor1;
        internal Vector2 m_groundAnchor2;
        internal Vector2 m_localAnchor1;
        internal Vector2 m_localAnchor2;

        internal Vector2 m_u1;
        internal Vector2 m_u2;

        internal float m_constant;
        internal float m_ratio;

        internal float m_maxLength1;
        internal float m_maxLength2;

        // Effective masses
        internal float m_pulleyMass;
        internal float m_limitMass1;
        internal float m_limitMass2;

        // Impulses for accumulation/warm starting.
        internal float m_impulse;
        internal float m_limitImpulse1;
        internal float m_limitImpulse2;

        internal LimitState m_state;
        internal LimitState m_limitState1;
        internal LimitState m_limitState2;
    }
}
